﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Box2D.XNA;

namespace Jemgine.Engine
{
	public class PhysicsModule : Module
	{
		public Box2D.XNA.World PhysicsSimulator;

        public List<ActorPhysics> Actors = new List<ActorPhysics>();

        ContactListener ContactListener = new ContactListener();
		Settings _settings;

		public PhysicsModule(Settings settings)
		{
			_settings = settings;
            //Reset();
		}

        class Mover
        {
            public Engine.Scripts.MoveEntity Originator;
            public Data.ScriptPayload Payload;
            public Data.EntityRecord Entity;
            public Data.Waypoint Destination;
            public int Delay;
        }

        List<Mover> Movers = new List<Mover>();

        List<Sensor> Sensors = new List<Sensor>();

        public void AddMover(Engine.Scripts.MoveEntity Node, Data.ScriptPayload Payload)
        {
            if (Node.ReferenceTerminals[0].Target is Data.EntityRecord)
            {
                Data.EntityRecord Entity = Node.ReferenceTerminals[0].GetTarget(Payload) as Data.EntityRecord;
                Mover Modify = null;

                foreach (Mover mover in Movers)
                    if (Object.ReferenceEquals(mover.Entity, Entity)) Modify = mover;
                if (Modify == null)
                {
                    Modify = new Mover { Entity = Entity as Data.EntityRecord };
                    Movers.Add(Modify);
                }

                Modify.Originator = Node;
                Modify.Payload = Payload;
                Modify.Destination = Node.ReferenceTerminals[1].GetTarget(Payload) as Data.Waypoint;
                Modify.Delay = 0;
                Node.OutputTerminals[0].Fire(Payload);
            }
        }

        public void ForgetMover(Data.ScriptNode Entity)
        {
            for (int i = 0; i < Movers.Count; )
            {
                if (Object.ReferenceEquals(Entity, Movers[i].Entity))
                    Movers.RemoveAt(i);
                else
                    ++i;
            }

        }

        public override void BeginSimulation()
		{
			ContactListener.Clear();
            Actors.Clear();
            Movers.Clear();
            Sensors.Clear();

			PhysicsSimulator = new Box2D.XNA.World(Vector2.Zero /*_settings.Gravity*/, true);
			PhysicsSimulator.ContinuousPhysics = true;
			PhysicsSimulator.WarmStarting = true;
			PhysicsSimulator.ContactListener = this.ContactListener;

			//base.Reset();
		}

		public class RayCastResult
		{
            public bool Hit = false;
			public Physics PhysicsComponent;
			public Vector2 Point;
			public Vector2 Normal;
		}

		float RayCastCallback(Fixture fixture, Vector2 point, Vector2 normal, float fraction, RayCastResult result,
            Func<Physics, bool> Filter)
		{
            if (Filter(fixture.GetBody().GetUserData() as Physics))
            {
                result.Hit = true;
                result.PhysicsComponent = fixture.GetBody().GetUserData() as Physics;
                result.Normal = normal;
                result.Point = point;
            }
            return fraction;
		}

		public RayCastResult RayCast(Vector2 Start, Vector2 End, Func<Physics, bool> Filter)
		{
			RayCastResult result = new RayCastResult();
			PhysicsSimulator.RayCast((A, B, C, D) => RayCastCallback(A, B, C, D, result, Filter), Start, End);
			return result;
		}

        public List<ActorPhysics> QueryActors(Jemgine.Math.OBB OBB, Func<ActorPhysics, bool> Filter)
        {
            List<ActorPhysics> Result = new List<ActorPhysics>();
            foreach (var Actor in Actors)
            {
                if (Jemgine.Math.OBB.Intersect(OBB, Actor.BoundingBox) && Filter(Actor))
                    Result.Add(Actor);
            }
            return Result;
        }

        public void CreateJoint(Data.Dynamic Joint)
        {
            Box2D.XNA.JointDef Def = Joint.GetJointDef();
            if (Def == null) return;
            Joint.Joint = PhysicsSimulator.CreateJoint(Def);
        }

        public void AddSensor(Sensor Sensor)
        {
            Sensors.Add(Sensor);
        }

        public override void Update(float ElapsedSeconds)
        {
            if (Sim.SupressPhysics) return;

            //Update movers
            for (int i = 0; i < Movers.Count; )
            {
                Mover Mover = Movers[i];
                if (Mover.Destination == null) { Movers.RemoveAt(i); continue; }
                if (Mover.Entity == null) { Movers.RemoveAt(i); continue; }

                if (Mover.Delay > 0)
                {
                    Mover.Delay -= 1;
                    ++i;
                    continue;
                }

                Physics Physics = Mover.Entity.GetComponent<Physics>();

                Vector2 DestinationPosition = Mover.Destination.Position;
                Vector2 Delta = DestinationPosition - Mover.Entity.Position;

                float TimeLeft = Delta.Length() / Mover.Originator.Speed;

                if (TimeLeft < 1.0f / 60.0f)
                {
                    if (Physics != null)
                    {
                        Physics.PhysicsBody.SetLinearVelocity(Vector2.Zero);
                        Physics.PhysicsBody.SetAngularVelocity(0.0f);
                        Physics.PhysicsBody.Position = Mover.Destination.Position;
                        Physics.PhysicsBody.Rotation = Mover.Destination.Angle;
                        Physics.PhysicsBody.SetAwake(true);
                    }
                    Mover.Entity.Position = Mover.Destination.Position;
                    Mover.Entity.Angle = Mover.Destination.Angle;
                    Mover.Delay = Mover.Destination.Pause;
                    Mover.Destination = Mover.Destination.ReferenceTerminals[0].Target as Data.Waypoint;

                    if (Mover.Destination == null)
                    {
                        Movers.RemoveAt(i);
                        Mover.Originator.OutputTerminals[1].Fire(Mover.Payload);
                    }
                    else
                        ++i;

                    continue;
                }
                else
                {
                    Delta.Normalize();
                    Delta *= Mover.Originator.Speed;

                    float AngleDelta = Jemgine.Math.Angle.Delta(Mover.Entity.Angle, Mover.Destination.Angle);
                    float AngularVelocity = AngleDelta / TimeLeft;

                    if (Physics != null)
                    {
                        Physics.PhysicsBody.SetLinearVelocity(Delta);
                        Physics.PhysicsBody.SetAngularVelocity(AngularVelocity);
                    }
                    else
                    {
                        Mover.Entity.Position += Delta / 60.0f;
                        Mover.Entity.Angle += AngularVelocity / 60.0f;
                    }
                }

                ++i;
            }

            foreach (var Sensor in Sensors)
                Sensor.SensorFire();

            PhysicsSimulator.Step(ElapsedSeconds, 10, 3);
            PhysicsSimulator.ClearForces();
            ProcessContacts();

            for (int A = 0; A < Actors.Count; ++A)
            {
                if (Actors[A].PreventRotation && !Jemgine.Math.Utility.AlmostZero(Actors[A].Gravity.LengthSquared()))
                {
                    float GravityAngle = -Jemgine.Math.Vector.AngleBetweenVectors(Actors[A].Gravity, new Vector2(0, -1));
                    GravityAngle = Jemgine.Math.Angle.Normalize(GravityAngle);
                    float Difference = Jemgine.Math.Angle.Delta(
                        Jemgine.Math.Angle.Normalize(Actors[A].PhysicsBody.Rotation), GravityAngle);
                    if (Difference > 0.2f) Difference = 0.2f;
                    if (Difference < -0.2f) Difference = -0.2f;
                    Actors[A].PhysicsBody.Rotation += Difference;
                    //Actors[A].PhysicsBody.Rotation = GravityAngle;
                }

                Actors[A].ProcessContacts();

                if (Actors[A].CrushForce > Actors[A].CrushThreshold)
                    Actors[A].OutputTerminals[1].Fire(
                        new Data.ScriptPayload { SenderRef = Actors[A].Me, Physics = Actors[A] });
            }
            

            if (Sim.MapHeader != null)
            {
                for (var Body = PhysicsSimulator.GetBodyList(); Body != null; Body = Body.GetNext())
                {
                    Physics Physics = Body.GetUserData() as Physics;
                    if (Physics != null) Physics.SetGravity(Sim.MapHeader.Gravity * Sim.MapHeader.GravityForce);
                    else Body.Gravity = Sim.MapHeader.Gravity * Sim.MapHeader.GravityForce;
                }
            }

          
            for (var Body = PhysicsSimulator.GetBodyList(); Body != null; Body = Body.GetNext())
            {
                Physics Component = Body.GetUserData() as Physics;
                if (Component.Me != null)
                {
                    Component.Me.Position = Body.Position;
                    Component.Me.Angle = Body.GetAngle();
                }
            }
        }

        void ProcessContacts()
		{

			foreach (ContactPoint cp in ContactListener._points)
			{
                Physics AP = cp.fixtureA.GetBody().GetUserData() as Physics;
                Physics BP = cp.fixtureB.GetBody().GetUserData() as Physics;

				if (AP == null || BP == null) throw new InvalidProgramException("How did this happen?");

                AP.OnContact(BP, cp.normal, cp.position);
				BP.OnContact(AP, -cp.normal, cp.position);
			}

			foreach (SensorContact sc in ContactListener.SensorContacts)
			{
				if (sc.Sensor == null || sc.Trigger == null) throw new InvalidProgramException("How did this happen?");

                bool Fire = true;

                PolygonPhysics PolyPhysics = sc.Sensor as PolygonPhysics;
                if (PolyPhysics != null && PolyPhysics.PhysicsType == PolygonPhysicsType.CenterSensor)
                    Fire = PolyPhysics.TestPoint(sc.Trigger.PhysicsBody.GetPosition());
                
				if (Fire) sc.Sensor.OnContact(sc.Trigger, Vector2.Zero, Vector2.Zero);
			}

			ContactListener._points.Clear();
		}
    }
}
